perm filename DIAGY.SAI[AL,HE] blob
sn#198174 filedate 1977-06-29 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00008 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11 'AL SERVO
C00004 00003 ⊃ PROCEDURES FOR CONVERTING TO AND FROM PDP11/45 NUMBER REPRESENTATION
C00006 00004 ⊃ START OF RUN TIME CODE
C00009 00005 ⊃ SEND COMMAND TO THE ELF. IF RUN IS NOT ZERO, SOMETHINGS WRONG WITH THE
C00011 00006 ⊃ TYPE OUT POSSIBLE DISPLAY MODES IS ASKED TO
C00014 00007 ⊃ TRANSFER THE DATA AND DISPLAY THE GRAPHS
C00021 00008 ⊃ IF P COMMAND, WRITE OUT A DISK FILE CONTAINING THE DISPLAY BUFFER.
C00023 ENDMK
C⊗;
COMMENT PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11 'AL SERVO;
BEGIN "DIAGY"
REQUIRE "TLKEF2.FAI" LOAD_MODULE;
DEFINE ⊃="COMMENT ",CRLF="('15&'12)",FUNCT="1",HEAD="2";
INTEGER ARM,DURA,DUM,JOINT,I,RUN,MASTER,K,J,ANG1,ANG2;
INTEGER LL,UL,POG,MODULUS,BP,DD,III;
INTEGER ARRAY DISPLY[1:'3000];
INTEGER ARRAY DATA[1:3601];
INTEGER ARRAY VEL[1:101],ANG[1:101],MOTOR[1:101];
INTEGER ARRAY TH[1:600],THD[1:600],TORQUE[1:600],STIME[1:600];
STRING WARM,ANS,LINED,GANS,COM1,COM2,COM3;
STRING DISCM;
STRING DANS,RELD;
REAL ANGLE;
LABEL AMPLP,RUNELF,LPEND,BOTTOM;
LABEL ARMLP,MDONE;
REQUIRE "DPYSUB.HDR[1,PDQ]" SOURCE_FILE;
EXTERNAL INTEGER PROCEDURE TLKEF2(INTEGER ARM,JOINT,DURA,ANGLE1,ANGLE2,
MASTER;INTEGER ARRAY DATA);
⊃ PROCEDURES FOR CONVERTING TO AND FROM PDP11/45 NUMBER REPRESENTATION;
REAL PROCEDURE GETFLT(INTEGER NUM1,NUM2);
BEGIN
INTEGER LBYTE;
REAL Z;
LABEL DNE;
LBYTE←'142700000002;
START_CODE
MOVE 1,NUM1;
LSH 1,16;
MOVE 2,NUM2;
ANDI 2,'177777;
IOR 1,2;
SETZ 2,;
DPB 1,LBYTE;
LSH 1,-23;
LSHC 1,-9;
JUMPE 2,DNE;
TLO 2,'400;
JUMPGE 2,DNE;
TLZ 2,'400000;
MOVN 2,2;
DNE: MOVEM 2,Z;
END;
RETURN(Z);
END;
PROCEDURE CONELF(REFERENCE INTEGER NUM1,NUM2;REAL T);
BEGIN
INTEGER BYTE;
LABEL ST1,ST2,OVER;
BYTE←'013200000002;
START_CODE
MOVE 2,T;
JUMPGE 2,ST1;
MOVN 2,2;
TLO 2,'400000;
ST1: JFCL 2,ST2;
ST2: ADDI 2,4;
JFCL 2,OVER;
DPB 2,BYTE;
SETZ 1,;
LSHC 1,16;
MOVEM 1,NUM1;
SETZ 1,;
LSHC 1,16;
MOVEM 1,NUM2;
END;
RETURN;
OVER: OUTSTR("ERROR-ROUNDING OVERFLOW"&CRLF);
RETURN;
END;
⊃ START OF RUN TIME CODE;
MASTER←'13131;
LINED←"1 -30 2000";
DANS←"D 0 2000";
SETBREAK(HEAD,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?"&'12&'15,NULL,"IRN");
SETBREAK(FUNCT,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?0123456789",NULL,"XNR");
OUTSTR(CRLF&CRLF&"'AL SERVO TEST"&CRLF);
SETFORMAT(0,0);
⊃ CHECK WHETHER RUNNING ON A III OR A DATA DISK DISPLAY;
START_CODE
DEFINE PPINFO= "'702240000000";
PPINFO @DATA;
MOVE 1,DATA;
HLRZ 2(1);
SETZM III;
TRNN '100000;
AOS III;
END;
⊃ READ IN THE ARM TO BE USED;
ARMLP: OUTSTR("Yellow OR Blue arm (Y,B) ? ");
ANS←INCHRW; CLRBUF; OUTSTR(CRLF);
IF EQU("Y",ANS) THEN BEGIN
WARM←"YELLOW ";
ARM←0;
END
ELSE IF EQU("B",ANS) THEN BEGIN
WARM←"BLUE ";
ARM←1;
END
ELSE GOTO MDONE;
WARM← WARM&"ARM ";
⊃ READ IN THE JOINT, CHANGE IN JOINT ANGLE, AND DURATION;
AMPLP: OUTSTR("Joint,Angle,Duration(msec) "
&CRLF);
LODED(LINED&'15);
LINED←ANS←INCHWL;
JOINT←INTSCAN(ANS,DUM);
ANGLE←REALSCAN(ANS,DUM);
DURA←INTSCAN(ANS,DUM);
IF((JOINT<1)∨(JOINT>7)) THEN BEGIN
OUTSTR("ILLEGAL JOINT SPECIFICATION"&CRLF);
GO TO ARMLP;
END;
COM1← WARM&"JOINT # "&CVS(JOINT);
IF((DURA<1)∨(DURA>32000)) THEN BEGIN
OUTSTR("ILLEGAL DRIVE DURATION"&CRLF);
GO TO ARMLP;
END;
⊃ SEND COMMAND TO THE ELF. IF RUN IS NOT ZERO, SOMETHINGS WRONG WITH THE
ELF;
RUNELF: CONELF(ANG1,ANG2,ANGLE);
RUN←TLKEF2(ARM,JOINT,DURA,ANG1,ANG2,MASTER,DATA);
IF RUN=1 THEN GOTO AMPLP
ELSE IF RUN≠0 THEN OUTSTR("ARM STATUS = "&CVOS(-RUN)&CRLF);
OUTSTR("Do you want graphics now (Y,N)? ");
GANS←INCHRW; CLRBUF; OUTSTR(CRLF);
IF ¬EQU("Y",GANS) THEN GOTO AMPLP;
⊃ DECODE THE SERVO DATA;
IF DATA[1]=0 THEN BEGIN
OUTSTR("NO DATA COLLECTED"&CRLF);
GOTO ARMLP;
END;
OUTSTR("Data Collection Period"&CRLF);
DD←0;
IF DATA[1]>3600 THEN DATA[1]←3600;
FOR J←2 STEP 6 UNTIL DATA[1] DO BEGIN
DD←DD+1;
STIME[DD]←DATA[J];
TH[DD]←GETFLT(DATA[J+1],DATA[J+2])*100.0;
THD[DD]←GETFLT(DATA[J+3],DATA[J+4])*10000.0;
TORQUE[DD]←DATA[J+5];
END;
OUTSTR(" Start Time ="&CVS(STIME[1])&", End Time ="&CVS(STIME[DD])
&CRLF);
⊃ READ IN THE LIMITS OF THE INTERVAL TO BE DISPLAYED;
DO BEGIN
LABEL BOTTOM,LPEND;
OUTSTR("Graph Function, Start Time, End Time:"&CRLF);
LODED(DANS&'15);
ANS←RELD←INCHWL;
SCAN(ANS,HEAD,DUM);
DISCM←SCAN(ANS,FUNCT,DUM);
⊃ TYPE OUT POSSIBLE DISPLAY MODES IS ASKED TO;
IF EQU(DISCM,"?") THEN BEGIN
OUTSTR("The possible functions are as follows:"&CRLF&
" C-Modify current graph comment line"&CRLF&
" D-Display pos,vel error & motor torque"&CRLF&
" E-Terminate display mode"&CRLF&
" J-Display joint angle "&CRLF&
" P-Output display buffer to disk"&CRLF&
" S-Save response data on disk"&CRLF&
" T-Display tachometer reading "&CRLF&
" ?-Explain possible functions"&CRLF);
END
⊃ MODIFY COMMENT;
ELSE IF EQU(DISCM,"C") THEN BEGIN
OUTSTR("TYPE IN A NEW COMMENT"&CRLF);
LODED(COM3&'15);
COM3←INCHWL & " ";
GOTO LPEND;
END
⊃ IF DISPLAY MODE, GET LOWER AND UPPER LIMITS OF TIME TO BE DISPLAYED.;
ELSE IF EQU(DISCM,"D")∨EQU(DISCM,"J")∨EQU(DISCM,"T") THEN BEGIN
LL←INTSCAN(ANS,DUM);
UL←INTSCAN(ANS,DUM);
IF (LL≥UL) THEN BEGIN
OUTSTR("ILLEGAL LIMITS"&CRLF);
GOTO BOTTOM
END;
IF (LL<STIME[1]) THEN LL←STIME[1];
IF (UL>STIME[DD]) THEN UL←STIME[DD];
⊃ INITIALIZE DISPLAY ROUTINES.;
DPYCLR;
POG←GETPOG;
DPYSET(DISPLY);
COM2←"START TIME = "&CVS(LL)&" FINAL TIME = "&CVS(UL);
⊃ DETERMINE THE STEP FACTOR TO BE USED IN PICKING OUT WHAT DATA IS TO
BE DISPLAYED, MODULUS.;
FOR I←1 STEP 1 UNTIL DD-1 DO BEGIN
IF (STIME[I]≤LL)∧(STIME[I+1]≥LL) THEN LL←I;
IF (STIME[I]≤UL)∧(STIME[I+1]≥UL) THEN UL←I;
END;
MODULUS← IF (UL-LL)≤100 THEN 1 ELSE (UL-LL)%100+1;
⊃ TRANSFER THE DATA AND DISPLAY THE GRAPHS;
BP←0;
IF EQU(DISCM,"D") THEN BEGIN
FOR I←LL STEP MODULUS UNTIL UL DO BEGIN
BP←BP+1;
MOTOR[BP]←TORQUE[I];
ANG[BP]←TH[I];
VEL[BP]←THD[I];
END;
ARRGRF(ANG,1,BP,-400,150,800,250,"T/"&CVS(MODULUS),
"POSITION ERROR 1/100 DEG, JOINT "&CVS(JOINT)&" FROM "
&CVS(LL)&" TO "&CVS(UL));
ARRGRF(VEL,1,BP,-400,-150,800,250,"T/"&CVS(MODULUS),
"VELOCITY ERROR 1/10000 DEG, JOINT "&CVS(JOINT)&" FROM "
&CVS(LL)&" TO "&CVS(UL));
ARRGRF(MOTOR,1,BP,-400,-450,800,250,"T/"&CVS(MODULUS),
"MOTOR, JOINT "&CVS(JOINT)&" FROM "&CVS(LL)&" TO "&CVS(UL));
END ELSE
IF EQU(DISCM,"J") THEN BEGIN
FOR I←LL STEP MODULUS UNTIL UL DO BEGIN
BP←BP+1;
ANG[BP]←TH[I];
END;
ARRGRF(ANG,1,BP,-300,-300,800,700,"T/"&CVS(MODULUS),
"JOINT ANGLE 1/100 DEG, JOINT "&CVS(JOINT)&" FROM "
&CVS(LL)&" TO "&CVS(UL));
END ELSE
IF EQU(DISCM,"T") THEN BEGIN
FOR I←LL STEP MODULUS UNTIL UL DO BEGIN
BP←BP+1;
VEL[BP]←THD[I];
END;
ARRGRF(VEL,1,BP,-300,-300,800,700,"T/"&CVS(MODULUS),
"JOINT VELOCITY 1/10000 DEG, JOINT "&CVS(JOINT)&" FROM "
&CVS(LL)&" TO "&CVS(UL));
END;
DPYBIG(2);
AIVECT(-400,475);
DPYSST(COM1);
AIVECT(-400,450);
DPYSST(COM2);
AIVECT(-400,425);
DPYSST(COM3);
DPYOUT(POG);
IF III≠0 THEN BEGIN
QUICK_CODE -8304460352; END; ⊃ DPYPOS -1100, makes PP invisible;
END;
INCHWL;
DPYCLR;
END
⊃ IF P COMMAND, WRITE OUT A DISK FILE CONTAINING THE DISPLAY BUFFER.;
ELSE IF EQU(DISCM,"P") THEN BEGIN
STRING FILNAM;
INTEGER FLG,CHN;
CHN←14;
OPEN(CHN,"DSK",8,0,3,0,0,0);
DO BEGIN
OUTSTR("PLOT FILE = ");
FILNAM←INCHWL;
ENTER(CHN,FILNAM&".PLT",FLG);
END UNTIL ¬FLG;
ARRYOUT(CHN,DISPLY[1],DISPLY[2]);
CLOSE(CHN);
RELEASE(CHN);
GOTO LPEND;
END
⊃ IF S COMMAND, ASK FOR COMMENT LINE AND SAVE DATA ON DISK.;
ELSE IF EQU(DISCM,"S") THEN BEGIN
STRING FILNAM,LST;
INTEGER FLG,CHN,I;
CHN←1;
SETFORMAT(12,6);
OPEN(CHN,"DSK",0,0,2,120,FLG,FLG);
DO BEGIN
OUTSTR("DATA FILE = ");
FILNAM←INCHWL;
ENTER(CHN,FILNAM&".DAT",FLG);
END UNTIL ¬FLG;
OUTSTR("COMMENT = "); LST←INCHWL;
OUT(CHN,LST&CRLF);
FOR I←1 STEP 1 UNTIL DD DO BEGIN
LST←CVS(STIME[I])&" "&CVE(TH[I]*0.01)&
" "&CVE(THD[I]*0.0001)&CRLF;
OUT(CHN,LST);
END;
CLOSE(CHN);
RELEASE(CHN);
SETFORMAT(0,0);
GOTO LPEND;
END
ELSE IF EQU(DISCM,"E") THEN GOTO LPEND
ELSE OUTSTR("ILLEGAL FUNCTION"&CRLF);
⊃ FINAL STEP OF DECODING GRAPHICS INSTRUCTIONS;
BOTTOM: DANS←RELD;
LPEND: END UNTIL EQU(DISCM,"E");
GOTO AMPLP;
MDONE: END